#include "total.h"

double wx;
double wy;
double wz;

double ax;
double ay;
double az;

double vx;
double vy;

double px;
double py;

float temper;
double last_x,last_y;
volatile uint8_t JY901_Angle[6];
volatile uint8_t JY901_Acc[6];
volatile uint8_t Temper[2];

void JY901_Get_Acc(void)
{
	
	static uint8_t state = 0;
	if(state==0 && Uart1_Receive_Data==Start_Sign)
	{
		state=1;
	}
	else if(state==1 && Uart1_Receive_Data==Start_Acc)
	{
		state=2;
	}
	else if(state==2)
	{
		state=3;
		JY901_Acc[0]=Uart1_Receive_Data;	//AxL
	}
	else if(state==3)
	{
		state=4;
		JY901_Acc[1]=Uart1_Receive_Data;	//AxH
	}
	else if(state==4)
	{
		state=5;
		JY901_Acc[2]=Uart1_Receive_Data;	//AyL
	}
	else if(state==5)
	{
		state=6;
		JY901_Acc[3]=Uart1_Receive_Data;	//AyH
	}
	else if(state==6)
	{
		state=7;
		JY901_Acc[4]=Uart1_Receive_Data;	//AzL
	}
	else if(state==7)
	{
		state=8;
		JY901_Acc[5]=Uart1_Receive_Data;	//AzH
	}
	else if(state==8)
	{
		state=9;
		Temper[0]=Uart1_Receive_Data;	//TL
	}
	else if(state==9)
	{
		state=10;
		Temper[1]=Uart1_Receive_Data;	//TH

	}
	else if(state==10)
	{
		if((Start_Sign+Start_Acc+
			JY901_Acc[0]+JY901_Acc[1]+JY901_Acc[2]+JY901_Acc[3]+JY901_Acc[4]+JY901_Acc[5]+
			Temper[0]+Temper[1])%256==Uart1_Receive_Data)
		{
			ax=(float)(((short)(JY901_Acc[1]<<8)|JY901_Acc[0])*16*G_acc)/32768;
			ay=(float)(((short)(JY901_Acc[3]<<8)|JY901_Acc[2])*16*G_acc)/32768;
			az=(float)(((short)(JY901_Acc[5]<<8)|JY901_Acc[4])*16*G_acc)/32768;
			temper=(float)((short)(Temper[1]<<8)|Temper[0])/100;
			ax+=sin(wy*3.14159/180)*sin(wx*3.14159/180)*G_acc;
			ay-=sin(wx*3.14159/180)*cos(wy*3.14159/180)*G_acc;
			
//			printf("wx=%.2f wy=%.2f wz=%.2f\r\n",wx,wy,wz);
			vx+=ax*0.01;
			vy+=ay*0.01;
			
			px+=vx*0.01;
			py+=vy*0.01;
			
//			pz+=(az-G_acc)*0.01;
			state=0;
		}
	}
	else
	{
		state = 0;
	}
	
}
void JY901_Get_Angle(void)
{
	static uint8_t state = 0;
	if(state==0 && Uart1_Receive_Data==Start_Sign)
	{
		state=1;
	}
	else if(state==1 && Uart1_Receive_Data==Get_Angle)
	{
		state=2;
	}
	else if(state==2)
	{
		state=3;
		JY901_Angle[0]=Uart1_Receive_Data;	//wxL
	}
	else if(state==3)
	{
		state=4;
		JY901_Angle[1]=Uart1_Receive_Data;	//wxH
	}
	else if(state==4)
	{
		state=5;
		JY901_Angle[2]=Uart1_Receive_Data;	//wyL
	}
	else if(state==5)
	{
		state=6;
		JY901_Angle[3]=Uart1_Receive_Data;	//wyH
	}
	else if(state==6)
	{
		state=7;
		JY901_Angle[4]=Uart1_Receive_Data;	//wzL
	}
	else if(state==7)
	{
		state=8;
		JY901_Angle[5]=Uart1_Receive_Data;	//wzH
	}
	else if(state==8)
	{
		state=9;
		Temper[0]=Uart1_Receive_Data;	//TL
	}
	else if(state==9)
	{
		state=10;
		Temper[1]=Uart1_Receive_Data;	//TH

	}
	else if(state==10)
	{
		if((Start_Sign+Get_Angle+
			JY901_Angle[0]+JY901_Angle[1]+JY901_Angle[2]+JY901_Angle[3]+JY901_Angle[4]+JY901_Angle[5]+
			Temper[0]+Temper[1])%256==Uart1_Receive_Data)
		{
			// wx=(float)(((short)(JY901_Angle[1]<<8)|JY901_Angle[0])*180)/32768;
			// wy=(float)(((short)(JY901_Angle[3]<<8)|JY901_Angle[2])*180)/32768;
			wz=(float)(((short)(JY901_Angle[5]<<8)|JY901_Angle[4])*180)/32768;
			temper=(float)((short)(Temper[1]<<8)|Temper[0])/100;
			printf("wx=%.2f wy=%.2f wz=%.2f\r\n",wx,wy,wz);
            Angle_PIDControl_Flag = true;
			state=0;
		}
	}
	else
	{
		state = 0;
	}
}
